from __future__ import division
import time
# pca9685 操作舵机
import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()

class servoContler(object):
    def __init__(self):
        # 下面舵机
        self.channel4 = int(4)
        # 上面舵机
        self.channel5 = int(5)
        pwm.set_pwm_freq(50)

        self.btm_kp = 2  # 底部舵机的Kp系数
        self.top_kp = 2  # 顶部舵机的Kp系数
        self.offset_dead_block = 0.1  # 设置偏移量的死区


    def set_cloud_platform_degree(self, last_btm_degree, last_top_degree):
        pwm.set_pwm(self.channel4, 0, int(4096 * ((last_btm_degree * 11) + 500) / 20000))
        pwm.set_pwm(self.channel5, 0, int(4096 * ((last_top_degree * 11) + 500) / 20000))
        time.sleep(0.1)

    def set_btm_cloud_platform_degree(self, last_btm_degree):
        pwm.set_pwm(self.channel4, 0, int(4096 * ((last_btm_degree * 11) + 500) / 20000))
        time.sleep(0.1)

    def set_top_cloud_platform_degree(self, last_top_degree):
        pwm.set_pwm(self.channel5, 0, int(4096 * ((last_top_degree * 11) + 500) / 20000))
        time.sleep(0.1)


    def btm_servo_control(self, offset_x, last_btm_degree):
        '''
        底部舵机的比例控制
        这里舵机使用开环控制
        '''
        # 设置最小阈值
        if abs(offset_x) < self.offset_dead_block:
            offset_x = 0

        # offset范围在-50到50左右
        delta_degree = offset_x * self.btm_kp
        # 计算得到新的底部舵机角度
        next_btm_degree = last_btm_degree + delta_degree
        # 添加边界检测
        if next_btm_degree < 0:
            next_btm_degree = 0
        elif next_btm_degree > 180:
            next_btm_degree = 180

        return int(next_btm_degree)

    def top_servo_control(self, offset_y, last_top_degree):
        '''
        顶部舵机的比例控制
        这里舵机使用开环控制
        '''
        # 如果偏移量小于阈值就不相应
        if abs(offset_y) < self.offset_dead_block:
            offset_y = 0

        # offset_y *= -1
        # offset范围在-50到50左右
        delta_degree = offset_y * self.top_kp
        # 新的顶部舵机角度
        next_top_degree = last_top_degree + delta_degree
        # 添加边界检测
        if next_top_degree < 0:
            next_top_degree = 0
        elif next_top_degree > 180:
            next_top_degree = 180

        return int(next_top_degree)

    def update_btm_kp(self, value):
        # 更新底部舵机的比例系数
        self.btm_kp = value

    def update_top_kp(self, value):
        # 更新顶部的比例系数
        self.top_kp = value

    def calculate_offset(self, img_width, img_height, face):
        '''
        计算人脸在画面中的偏移量
        偏移量的取值范围： [-1, 1]
        '''
        (x, y, w, h) = face
        face_x = float(x + w / 2.0)
        face_y = float(y + h / 2.0)
        # 人脸在画面中心X轴上的偏移量
        offset_x = float(face_x / img_width - 0.5) * 2
        # 人脸在画面中心Y轴上的偏移量
        offset_y = float(face_y / img_height - 0.5) * 2

        return (offset_x, offset_y)



    def set_servo_angle(self, channel, angle):
        date = 4096 * ((angle * 11) + 500) / 20000
        pwm.set_pwm(channel, 0, int(date))


if __name__ == '__main__':
    try:
        servo = servoContler()
        while True:
            channel = int(4)  # input('pleas input channel:')
            channel1 = int(5)  # input('pleas input channel:')
            angle = int(input('pleas input angle:'))
            servo.set_servo_angle(channel, angle)
            servo.set_servo_angle(channel1, angle)
            time.sleep(0.5)

    except KeyboardInterrupt:
        print("Measurement stopped by User")
